#include "ros/ros.h"
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d.hpp>

using namespace cv;
using namespace std;

void sobel(const cv::Mat &image) {
    // Sobel算子进行边缘检测
    Mat sobelX, sobelY, sobel;
    Sobel(image, sobelX, CV_32F, 1, 0);
    Sobel(image, sobelY, CV_32F, 0, 1);
    sobel = abs(sobelX) + abs(sobelY);

    // 显示原图和边缘检测结果
    imshow("Original Image", image);
    imshow("Sobel Edge Detection", sobel / 255.0);
    waitKey(0);
}

void keyPoint(const cv::Mat &image) {
    // Harris角点检测
    Mat harris;
    cornerHarris(image, harris, 2, 3, 0.04);
    Mat harris_norm;
    normalize(harris, harris_norm, 0, 255, NORM_MINMAX, CV_32FC1);
    Mat harris_result;
    convertScaleAbs(harris_norm, harris_result);

    // Shi-Tomasi角点检测
    vector<Point2f> corners;
    goodFeaturesToTrack(image, corners, 500, 0.01, 10);
    Mat shi_tomasi_result = image.clone();
    for (auto &corner: corners) {
        circle(shi_tomasi_result, corner, 3, Scalar(0, 255, 0), 2);
    }

    // FAST角点检测
    vector<KeyPoint> keypoints;
    FAST(image, keypoints, 10);
    Mat fast_result;
    drawKeypoints(image, keypoints, fast_result, Scalar(0, 0, 255));

    // SIFT角点检测
//    Ptr<Feature2D> sift = SIFT::create(0, 3, 0.04, 10);
//    vector<KeyPoint> keypoints_sift;
//    sift->detect(image, keypoints_sift);
//    Mat sift_result;
//    drawKeypoints(image, keypoints_sift, sift_result, Scalar(255, 0, 0));

    // 显示原图和角点检测结果
    imshow("Original Image", image);
    imshow("Harris Corner Detection", harris_result);
    imshow("Shi-Tomasi Corner Detection", shi_tomasi_result);
    imshow("FAST Corner Detection", fast_result);
//    imshow("SIFT Corner Detection", sift_result);
    waitKey(0);
}

void contour(const cv::Mat &image) {
//    // 将图像转换为灰度图像
//    Mat gray;
//    cvtColor(image, gray, COLOR_BGR2GRAY);

    // 对灰度图像进行二值化处理
    Mat binary;
    threshold(image, binary, 0, 255, THRESH_BINARY | THRESH_OTSU);

    // 进行形态学处理，去除噪声和连接断裂的边缘
    Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
    morphologyEx(binary, binary, MORPH_CLOSE, kernel);

    // 提取轮廓
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(binary, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

    // 绘制轮廓
    Mat result = image.clone();
    drawContours(result, contours, -1, Scalar(128), 2);

    // 显示原图和轮廓提取结果imshow("Original Image", image);
    imshow("Contour Detection", result);
    waitKey(0);
}

int main(int argc, char **argv) {

    ros::init(argc, argv, "cre_image");

//    // 创建一个 640x480 的纯白色图像
//    Mat circleImg(480, 640, CV_8UC3, Scalar(255, 255, 255));
//    // 在图像上画一个红色的圆
//    circle(circleImg, Point(320, 240), 100, Scalar(0, 0, 255), 2);
//    // 保存图像
//    imwrite("circleImg.png", circleImg);
//
//    // 创建一个 640x480 的灰度图像
//    Mat rectangleImg(480, 640, CV_8UC1, Scalar(128));
//    // 在图像上画一个白色的矩形
//    rectangle(rectangleImg, Point(100, 100), Point(200, 200), Scalar(255), 2);
//    // 保存图像
//    imwrite("rectangleImg.jpg", rectangleImg);
//
//    // 读取一张灰度图像
//    Mat img = imread("test.jpg", IMREAD_GRAYSCALE);
//
//    // 遍历图像中的所有像素
//    for (int i = 0; i < img.rows; i++) {
//        for (int j = 0; j < img.cols; j++) {
//            // 获取当前像素的灰度值
//            uchar gray = img.at<uchar>(i, j);
//
//            // 将像素值除以2
//            img.at<uchar>(i, j) = gray / 2;
//        }
//    }
//
//    // 保存修改后的图像
//    imwrite("test_modified.jpg", img);

    // 读取图像
    Mat image = imread("../../../src/cre/maps/sim_mymap.pgm", IMREAD_GRAYSCALE);

//    sobel(image.clone());
//    keyPoint(image.clone());
    contour(image.clone());

    ros::MultiThreadedSpinner spinner;
    spinner.spin();

    return 0;
}


